/* Copyright 2018 The Apollo Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "modules/canbus/vehicle/agilex/agilex_controller.h"
#include <cmath>

#include "modules/common/proto/vehicle_signal.pb.h"

#include "modules/canbus/vehicle/agilex/agilex_message_manager.h"
#include "modules/canbus/vehicle/vehicle_controller.h"
#include "modules/common/log.h"
#include "modules/common/time/time.h"
#include "modules/drivers/canbus/can_comm/can_sender.h"
#include "modules/drivers/canbus/can_comm/protocol_data.h"

DEFINE_double(agilex_steering_coefficient, 1.0, "Ratio of steering target");
DEFINE_double(agilex_speed_coefficient, 1.0, "Ratio of steering target");

namespace apollo {
namespace canbus {
namespace agilex {

using ::apollo::common::ErrorCode;
using ::apollo::control::ControlCommand;
using ::apollo::drivers::canbus::ProtocolData;

namespace {

const int32_t kMaxFailAttempt = 10;
const int32_t CHECK_RESPONSE_STEER_UNIT_FLAG = 1;
const int32_t CHECK_RESPONSE_SPEED_UNIT_FLAG = 2;
}  // namespace

ErrorCode AgileXController::Init(
    const VehicleParameter& params,
    CanSender<::apollo::canbus::ChassisDetail>* const can_sender,
    MessageManager<::apollo::canbus::ChassisDetail>* const message_manager) {
  if (is_initialized_) {
    AINFO << "AgileXController has already been initiated.";
    return ErrorCode::CANBUS_ERROR;
  }
  vehicle_params_.CopyFrom(
      common::VehicleConfigHelper::instance()->GetConfig().vehicle_param());
  params_.CopyFrom(params);
  if (!params_.has_driving_mode()) {
    AERROR << "Vehicle conf pb not set driving_mode.";
    return ErrorCode::CANBUS_ERROR;
  }

  if (can_sender == nullptr) {
    return ErrorCode::CANBUS_ERROR;
  }
  can_sender_ = can_sender;

  if (message_manager == nullptr) {
    AERROR << "protocol manager is null.";
    return ErrorCode::CANBUS_ERROR;
  }
  message_manager_ = message_manager;

  // sender part
  accel_cmd_130_ = dynamic_cast<Accelcmd130*>(
      message_manager_->GetMutableProtocolDataById(Accelcmd130::ID));
  if (accel_cmd_130_ == nullptr) {
    AERROR << "Accelcmd130 does not exist in the AgileXMessageManager!";
    return ErrorCode::CANBUS_ERROR;
  }

  can_sender_->AddMessage(Accelcmd130::ID, accel_cmd_130_, false);

  // need sleep to ensure all messages received
  AINFO << "AgileXController is initialized.";
  accel_cmd_130_->set_option_cmd(0x01);

  set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE);

  is_initialized_ = true;
  return ErrorCode::OK;
}

AgileXController::~AgileXController() {}

bool AgileXController::Start() {
  if (!is_initialized_) {
    AERROR << "AgileXController has NOT been initiated.";
    return false;
  }
  const auto& update_func = [this] { SecurityDogThreadFunc(); };
  thread_.reset(new std::thread(update_func));

  return true;
}

void AgileXController::Stop() {
  if (!is_initialized_) {
    AERROR << "AgileXController stops or starts improperly!";
    return;
  }

  if (thread_ != nullptr && thread_->joinable()) {
    thread_->join();
    thread_.reset();
    AINFO << "AgileXController stopped.";
  }
}

Chassis AgileXController::chassis() {
  chassis_.Clear();

  ChassisDetail chassis_detail;
  message_manager_->GetSensorData(&chassis_detail);

  // 21, 22, previously 1, 2
  if (driving_mode() == Chassis::EMERGENCY_MODE) {
    set_chassis_error_code(Chassis::NO_ERROR);
  }

  chassis_.set_driving_mode(driving_mode());
  chassis_.set_error_code(chassis_error_code());

  // 3
  chassis_.set_engine_started(true);

  // 5
  if (chassis_detail.agilex().has_accel_rpt_131() &&
      chassis_detail.agilex().accel_rpt_131().has_speed_mps()) {
    if (chassis_detail.agilex().accel_rpt_131().speed_mps() > 0.0) {
      chassis_.set_speed_mps(
          chassis_detail.agilex().accel_rpt_131().speed_mps());
      chassis_.set_throttle_percentage(100 *
          chassis_detail.agilex().accel_rpt_131().speed_mps() / 1.6);
      chassis_.set_brake_percentage(0);
    } else {
      chassis_.set_speed_mps(
          chassis_detail.agilex().accel_rpt_131().speed_mps() * -1.0);
      chassis_.set_throttle_percentage(
          -chassis_detail.agilex().accel_rpt_131().speed_mps() / 1.6);
      chassis_.set_brake_percentage(0);
    }
  } else {
    chassis_.set_speed_mps(0);
    chassis_.set_throttle_percentage(0);
    chassis_.set_brake_percentage(100);
  }

  // 7
  chassis_.set_fuel_range_m(0);

  // 23, previously 10
  chassis_.set_gear_location(Chassis::GEAR_DRIVE);

  // 11
  // TODO(QiL) : verify the unit here.
  if (chassis_detail.agilex().has_accel_rpt_131() &&
      chassis_detail.agilex().accel_rpt_131().has_steering_angular()) {
    chassis_.set_steering_percentage(
      chassis_detail.agilex().accel_rpt_131().steering_angular() / 0.751 * 100);
  } else {
    chassis_.set_steering_percentage(0);
  }

  chassis_.set_driving_mode(/*Chassis::COMPLETE_AUTO_DRIVE*/driving_mode());
  chassis_.mutable_engage_advice()->set_advice(
	  apollo::common::EngageAdvice::READY_TO_ENGAGE);
  // TODO(QiL) : implement the turn light signal here
  chassis_.mutable_signal()->set_turn_signal(
                              common::VehicleSignal::TURN_NONE);
  chassis_.set_chassis_error_mask(chassis_error_mask_);
  chassis_.set_engine_rpm(0);
  chassis_.set_odometer_m(0);
  chassis_.set_steering_torque_nm(0);
  chassis_.set_parking_brake(false);
  chassis_.set_high_beam_signal(false);
  chassis_.set_low_beam_signal(false);
  chassis_.set_left_turn_signal(false);
  chassis_.set_right_turn_signal(false);
  chassis_.set_horn(false);
  return chassis_;
}

void AgileXController::Emergency() {
  set_driving_mode(Chassis::EMERGENCY_MODE);
  ResetProtocol();
  set_chassis_error_code(Chassis::CHASSIS_ERROR);
}

ErrorCode AgileXController::EnableAutoMode() {
  if (driving_mode() == Chassis::COMPLETE_AUTO_DRIVE) {
    AINFO << "already in COMPLETE_AUTO_DRIVE mode";
    return ErrorCode::OK;
  }

  accel_cmd_130_->set_option_cmd(0x01);

  set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE);
  AINFO << "Switch to COMPLETE_AUTO_DRIVE mode ok.";
  return ErrorCode::OK;
}

ErrorCode AgileXController::DisableAutoMode() {
  ResetProtocol();
  can_sender_->Update();
  set_driving_mode(Chassis::COMPLETE_MANUAL);
  set_chassis_error_code(Chassis::NO_ERROR);
  AINFO << "Switch to COMPLETE_MANUAL ok.";
  return ErrorCode::OK;
}

ErrorCode AgileXController::EnableSteeringOnlyMode() {
  AFATAL << "Not supported!";
  return ErrorCode::OK;
}

ErrorCode AgileXController::EnableSpeedOnlyMode() {
  AFATAL << "Not supported!";
  return ErrorCode::OK;
}

// NEUTRAL, REVERSE, DRIVE
void AgileXController::Gear(Chassis::GearPosition gear_position) {
  if (!(driving_mode() == Chassis::COMPLETE_AUTO_DRIVE ||
        driving_mode() == Chassis::AUTO_SPEED_ONLY)) {
    AINFO << "this drive mode no need to set gear.";
    return;
  }
}

// brake with new acceleration
// acceleration:0.00~99.99, unit:
// acceleration:0.0 ~ 7.0, unit:m/s^2
// acceleration_spd:60 ~ 100, suggest: 90
// -> pedal
void AgileXController::Brake(double pedal) {
  // double real_value = params_.max_acc() * acceleration / 100;
  // TODO(QiL) Update brake value based on mode
  if (!(driving_mode() == Chassis::COMPLETE_AUTO_DRIVE ||
        driving_mode() == Chassis::AUTO_SPEED_ONLY)) {
    AINFO << "The current drive mode does not need to set acceleration.";
    return;
  }

  if (pedal > 10.) {
    accel_cmd_130_->set_accel_cmd(0.0);
    accel_cmd_130_->set_steer_cmd(0.0);
  }
}

// drive with old acceleration
// gas:0.00~99.99 unit:
void AgileXController::Throttle(double pedal) {
  if (!(driving_mode() == Chassis::COMPLETE_AUTO_DRIVE ||
        driving_mode() == Chassis::AUTO_SPEED_ONLY)) {
    AINFO << "The current drive mode does not need to set acceleration.";
    return;
  }

  accel_cmd_130_->set_accel_cmd(FLAGS_agilex_speed_coefficient * pedal);
}

void AgileXController::Acceleration(double acc) {}

// agilex default, -470 ~ 470, left:+, right:-
// need to be compatible with control module, so reverse
// steering with old angle speed
// angle:-99.99~0.00~99.99, unit:, left:-, right:+
void AgileXController::Steer(double angle) {
  if (!(driving_mode() == Chassis::COMPLETE_AUTO_DRIVE ||
        driving_mode() == Chassis::AUTO_STEER_ONLY)) {
    AINFO << "The current driving mode does not need to set steer.";
    return;
  }

  const double real_angle = vehicle_params_.max_steer_angle() * angle * 100.0;

  accel_cmd_130_->set_steer_cmd(FLAGS_agilex_steering_coefficient * angle);
}

// steering with new angle speed
// angle:-99.99~0.00~99.99, unit:, left:-, right:+
// angle_spd:0.00~99.99, unit:deg/s
void AgileXController::Steer(double angle, double angle_spd) {
  if (!(driving_mode() == Chassis::COMPLETE_AUTO_DRIVE ||
        driving_mode() == Chassis::AUTO_STEER_ONLY)) {
    AINFO << "The current driving mode does not need to set steer.";
    return;
  }
  const double real_angle = vehicle_params_.max_steer_angle() * angle * 100.0;

  const double real_angle_spd =
      ProtocolData<::apollo::canbus::ChassisDetail>::BoundedValue(
          vehicle_params_.min_steer_angle_rate(),
          vehicle_params_.max_steer_angle_rate(),
          vehicle_params_.max_steer_angle_rate() * angle_spd * 100.0);
  accel_cmd_130_->set_steer_cmd(FLAGS_agilex_steering_coefficient * angle);
}

void AgileXController::SetEpbBreak(const ControlCommand& command) {
  if (command.parking_brake()) {
    // None
  } else {
    // None
  }
}

void AgileXController::SetBeam(const ControlCommand& command) {
  if (command.signal().high_beam()) {
    // None
  } else if (command.signal().low_beam()) {
    // None
  } else {
    // None
  }
}

void AgileXController::SetHorn(const ControlCommand& command) {
  if (command.signal().horn()) {
    // None
  } else {
    // None
  }
}

void AgileXController::SetTurningSignal(const ControlCommand& command) {
  // Set Turn Signal
  auto signal = command.signal().turn_signal();
  if (signal == common::VehicleSignal::TURN_LEFT) {
    //;
  } else if (signal == common::VehicleSignal::TURN_RIGHT) {
    //;
  } else {
    //;
  }
}

void AgileXController::ResetProtocol() {
  message_manager_->ResetSendMessages();
}

bool AgileXController::CheckChassisError() {
  // TODO(QiL) : implement it here
  return false;
}

void AgileXController::SecurityDogThreadFunc() {
  int32_t vertical_ctrl_fail = 0;
  int32_t horizontal_ctrl_fail = 0;

  if (can_sender_ == nullptr) {
    AERROR << "Fail to run SecurityDogThreadFunc() because can_sender_ is "
              "nullptr.";
    return;
  }
  while (!can_sender_->IsRunning()) {
    std::this_thread::yield();
  }

  std::chrono::duration<double, std::micro> default_period{50000};
  int64_t start = 0;
  int64_t end = 0;
  while (can_sender_->IsRunning()) {
    start = ::apollo::common::time::AsInt64<::apollo::common::time::micros>(
        ::apollo::common::time::Clock::Now());
    const Chassis::DrivingMode mode = driving_mode();
    bool emergency_mode = false;

    // 1. horizontal control check
    if ((mode == Chassis::COMPLETE_AUTO_DRIVE ||
         mode == Chassis::AUTO_STEER_ONLY) &&
        CheckResponse(CHECK_RESPONSE_STEER_UNIT_FLAG, false) == false) {
      ++horizontal_ctrl_fail;
      if (horizontal_ctrl_fail >= kMaxFailAttempt) {
        emergency_mode = true;
        set_chassis_error_code(Chassis::MANUAL_INTERVENTION);
      }
    } else {
      horizontal_ctrl_fail = 0;
    }

    // 2. vertical control check
    if ((mode == Chassis::COMPLETE_AUTO_DRIVE ||
         mode == Chassis::AUTO_SPEED_ONLY) &&
        CheckResponse(CHECK_RESPONSE_SPEED_UNIT_FLAG, false) == false) {
      ++vertical_ctrl_fail;
      if (vertical_ctrl_fail >= kMaxFailAttempt) {
        emergency_mode = true;
        set_chassis_error_code(Chassis::MANUAL_INTERVENTION);
      }
    } else {
      vertical_ctrl_fail = 0;
    }
    if (CheckChassisError()) {
      set_chassis_error_code(Chassis::CHASSIS_ERROR);
      emergency_mode = true;
    }

    if (emergency_mode && mode != Chassis::EMERGENCY_MODE) {
      set_driving_mode(Chassis::EMERGENCY_MODE);
      message_manager_->ResetSendMessages();
    }
    end = ::apollo::common::time::AsInt64<::apollo::common::time::micros>(
        ::apollo::common::time::Clock::Now());
    std::chrono::duration<double, std::micro> elapsed{end - start};
    if (elapsed < default_period) {
      std::this_thread::sleep_for(default_period - elapsed);
    } else {
      AERROR << "Too much time consumption in AgileXController looping process:"
             << elapsed.count();
    }
  }
}

bool AgileXController::CheckResponse(const int32_t flags, bool need_wait) {
  /* ADD YOUR OWN CAR CHASSIS OPERATION
   */
  return true;
}

void AgileXController::set_chassis_error_mask(const int32_t mask) {
  std::lock_guard<std::mutex> lock(chassis_mask_mutex_);
  chassis_error_mask_ = mask;
}

int32_t AgileXController::chassis_error_mask() {
  std::lock_guard<std::mutex> lock(chassis_mask_mutex_);
  return chassis_error_mask_;
}

Chassis::ErrorCode AgileXController::chassis_error_code() {
  std::lock_guard<std::mutex> lock(chassis_error_code_mutex_);
  return chassis_error_code_;
}

void AgileXController::set_chassis_error_code(
    const Chassis::ErrorCode& error_code) {
  std::lock_guard<std::mutex> lock(chassis_error_code_mutex_);
  chassis_error_code_ = error_code;
}

}  // namespace agilex
}  // namespace canbus
}  // namespace apollo
